#include <stdlib.h>
#include <inttypes.h>
#include <arpa/inet.h>
#include <string.h>
#include "packet.h"

status_t packet_new(packet_t **packet, 
		uint8_t rid,
		uint8_t reserved, 
		uint8_t did,
		uint8_t length,
		uint8_t *data){
	
	*packet = (packet_t *) malloc (sizeof(packet_t));

	if (*packet == NULL){
		return OUT_OF_MEMORY;
	}

	packet_set_stx(*packet, STX);
	packet_set_rid(*packet, rid);
	packet_set_reserved(*packet, reserved);
	packet_set_did(*packet, did);
	packet_set_data(*packet, length, data);
	packet_set_checksum(*packet);
	packet_set_etx(*packet, ETX);

	return SUCCESS;
}

status_t packet_copy(packet_t **new, packet_t *orig)
{
	status_t retval = -1;

	retval = packet_new(new, orig->rid,
			orig->reserved,
			orig->did,
			orig->length,
			orig->data);

	return retval;
}

status_t packet_delete(packet_t *packet)
{
	if (packet == NULL || packet->data == NULL){
		return NULL_POINTER;
	}

	free(packet->data);
	free(packet);

	return SUCCESS;	
}

status_t packet_set_stx(packet_t *packet, uint16_t stx)
{
	if (packet == NULL){
		return NULL_POINTER;
	}

	packet->stx = stx;

	return SUCCESS;	
}

status_t packet_set_etx(packet_t *packet, uint16_t etx)
{
	if (packet == NULL){
		return NULL_POINTER;
	}

	packet->etx = etx;

	return SUCCESS;	
}

status_t packet_set_rid(packet_t *packet, uint8_t rid)
{
	if (packet == NULL){
		return NULL_POINTER;
	}

	packet->rid = rid;

	return SUCCESS;
}

status_t packet_set_reserved(packet_t *packet, uint8_t reserved)
{
	if (packet == NULL){
		return NULL_POINTER;
	}

	packet->reserved = reserved;

	return SUCCESS;
}

status_t packet_set_did(packet_t *packet, uint8_t did)
{
	if (packet == NULL){
		return NULL_POINTER;
	}

	packet->did = did;

	return SUCCESS;
}

status_t packet_set_data(packet_t *packet, 
			uint8_t length,
			uint8_t *data)
{
	int i = 0;

	if (packet == NULL){
		return NULL_POINTER;
	}

	packet->length = length;
	packet->data = (uint8_t *) malloc (sizeof(uint8_t) * length);

	if (packet->data == NULL){	
		return OUT_OF_MEMORY;
	}

	for (i=0; i<length; i++){
		packet->data[i] = data[i];
	}

	return SUCCESS;
}

status_t packet_set_checksum(packet_t *packet)
{
	if (packet == NULL){
		return NULL_POINTER;
	}

//	crc = crc_update(crc, packet->rid);
//	crc = crc_update(crc, packet->reserved);
//	crc = crc_update(crc, packet->did);
//	crc = crc_update(crc, packet->length);
//
//	for (i = 0; i < size; i++){
//		crc = crc_update(crc, packet->data[i]);
//	}

	packet_crc_calculate(&packet->checksum,
			packet->rid,
			packet->reserved,
			packet->did,
			packet->length,
			packet->data);


	return SUCCESS;
}

status_t packet_get_stx(packet_t *packet, uint16_t *stx)
{
	if (packet == NULL || stx == NULL){
		return NULL_POINTER;
	}

	*stx = packet->stx;

	return SUCCESS;
}

status_t packet_get_etx(packet_t *packet, uint16_t *etx)
{
	if (packet == NULL || etx == NULL){
		return NULL_POINTER;
	}

	*etx = packet->etx;

	return SUCCESS;
}

status_t packet_get_rid(packet_t *packet, uint8_t *rid)
{
	if (packet == NULL || rid == NULL){
		return NULL_POINTER;
	}

	*rid = packet->rid;

	return SUCCESS;
}

status_t packet_get_reserved(packet_t *packet, uint8_t *reserved)
{
	if (packet == NULL || reserved == NULL){
		return NULL_POINTER;
	}

	*reserved = packet->reserved;
	
	return SUCCESS;
}

status_t packet_get_did(packet_t *packet, uint8_t *did)
{
	if (packet == NULL || did == NULL){
		return NULL_POINTER;
	}

	*did = packet->did;

	return SUCCESS;
}

status_t packet_get_data(packet_t *packet, 
		       uint8_t *length,
		       uint8_t **data)
{
	if (packet == NULL || length == NULL){
		return NULL_POINTER;
	}

	*length = packet->length;
	*data = packet->data;
	//*data = (uint8_t *) malloc (sizeof(uint8_t) * packet->length);
	//memcpy(*data, packet->data, packet->length);

	return SUCCESS;
}

status_t packet_get_checksum(packet_t *packet, uint8_t *checksum)
{
	if (packet == NULL || checksum == NULL){
		return NULL_POINTER;
	}

	*checksum = packet->checksum;

	return SUCCESS;
}

status_t packet_serialize(packet_t *packet, uint8_t *stream)
{
	//stx,rid,rsv,did,len,data,chs,etx
	int i = 0; 

	if (packet == NULL){
		return NULL_POINTER;
	}

	memset(stream, 0, MAX_PACKET_LEN);
	
	(stream)[0] = (uint8_t) htons(packet->stx);
	(stream)[1] = (uint8_t) (htons(packet->stx) >> 8);
	(stream)[2] = packet->rid;
	(stream)[3] = packet->reserved;
	(stream)[4] = packet->did;
	(stream)[5] = packet->length;
	for (i=0; i<packet->length; i++){
		(stream)[i+6] = packet->data[i];	
	}
	(stream)[6+packet->length] = packet->checksum;
	(stream)[7+packet->length] = (uint8_t) htons(packet->etx);
	(stream)[8+packet->length] = (uint8_t) (htons(packet->etx) >> 8);

	return SUCCESS;
}

status_t packet_deserialize(packet_t **packet, uint8_t *stream)
{
	if (stream == NULL){
		return NULL_POINTER;
	}

	*packet = (packet_t *) malloc (sizeof(packet_t));

	if (*packet == NULL){
		return OUT_OF_MEMORY;
	}

	packet_set_stx(*packet, ntohs((stream[1]<<8)+stream[0]));
	packet_set_rid(*packet, stream[2]);
	packet_set_reserved(*packet, stream[3]);
	packet_set_did(*packet, stream[4]);
	packet_set_data(*packet, stream[5], &stream[6]);
//	packet_set_checksum(*packet, stream[6+stream[5]]);
//	must compare checksums to see if they match!
	packet_set_checksum(*packet);
	packet_set_etx(*packet, ntohs((stream[8+stream[5]]<<8)+
						stream[7+stream[5]]));
	
	return SUCCESS;
}

status_t packet_crc_calculate(uint8_t *crc, 
		uint8_t rid, uint8_t res, uint8_t did, uint8_t len,
		uint8_t *data)
{
	uint8_t i = 0;

	if ( NULL == data ) {
		return NULL_POINTER;
	}

	*crc = 0;

	*crc = crc_update(*crc, rid);
	*crc = crc_update(*crc, res);
	*crc = crc_update(*crc, did);
	*crc = crc_update(*crc, len);

	for (i = 0; i < len; i++){
		*crc = crc_update(*crc, data[i]);
	}

	return SUCCESS;	
}

status_t packet_are_equal(uint8_t *res, packet_t *packet1, packet_t *packet2)
{
	uint8_t tmp = 0x01, i = 0, size = 0;

	if ( NULL == packet1 || NULL == packet2 ) {
		return NULL_POINTER;
	}

	tmp &= ( packet1->stx == packet2->stx );	
	tmp &= ( packet1->etx == packet2->etx );	
	tmp &= ( packet1->rid == packet2->rid );	
	tmp &= ( packet1->reserved == packet2->reserved );	
	tmp &= ( packet1->did == packet2->did );	
	tmp &= ( packet1->length == packet2->length );
	tmp &= ( packet1->checksum == packet2->checksum );	

	size = packet1->length;

	while ( (0x01 == tmp) && (i < size) ) {
		tmp &= ( packet1->data[i] == packet2->data[i] );	
		i++;
	}

	*res = tmp;

	return SUCCESS;
}

uint16_t packet_stream_length(uint8_t *stream)
{
	return stream[5] + 9;
}


void print_packet_stream(FILE *fstream, uint8_t *ptr)
{
	uint8_t i = 0, len = ptr[INDEX_LEN] + INDEX_DAT + 3;

	fprintf(fstream, "\nPacket #:%d; Size:%d;\n", 
				ptr[INDEX_RSV], ptr[INDEX_LEN]);

	switch ( ptr[INDEX_DID] ) {
		case MOT_POS : 
			fprintf(fstream, "DC Motor move to Position\n");
			break;
		case MOT_POS_ALL : 
			fprintf(fstream, "DC Motor move all to position\n");
			break;
		case MOT_PWM : 
			fprintf(fstream, "DC Motor PWM control\n"); 
			break;
		case MOT_PWM_ALL : 
			fprintf(fstream, "DC Motor PWM controll all\n");
			break;
		case SETUP :
			fprintf(fstream, "System Settings:\n");
			break;
		case MOT_STATUS :
			fprintf(fstream, "DC Motor State: %s\n", 
				ptr[INDEX_DAT] ? "on" : "off" );
			break;
		case MOT_FB : 
			fprintf(fstream, "Motor Signal Feedback\n");
			break;
		case CUSTOM_AD : 
			fprintf(fstream, "Custom IO/AD Data Feedback\n");
			break;
		case SENSOR_FB : 
			fprintf(fstream, "Sensor Data Feedback\n");
			break;
		case SENSOR_FB_ALL : 
			fprintf(fstream, "All Sensor Feerback\n");
			break;
		case JPEG_PACKET:
			fprintf(fstream, "JPEG Packet\n");
			break;
		case SYSTEM_COM : 
			if ((RID_PMS5005 == ptr[INDEX_RID]) &&
				0x01 == ptr[INDEX_LEN] &&
				0x01 == ptr[INDEX_DAT] ) {
				fprintf(fstream, 
					"Ping Packet from Host to PMS5005\n");
				break;
			}

			if ( 0x01 == ptr[INDEX_LEN] && 
				0x01 == ptr[INDEX_DAT] ) {
				fprintf(fstream, 
					"Acknowledge Packet from PMS5005\n");
				break;
			}
			if ( 0x01 == ptr[INDEX_LEN] && 
				0x00 == ptr[INDEX_DAT] ) {
				fprintf(fstream, 
					"Ping Packet from PMS5005\n");
				break;
			}

		default : 
			fprintf(fstream, "Unknown Packet\n");
	}

	for ( i = 0; i< len; i++ ) {
		if ( (i+1)%16 == 0 ) {
			fprintf(fstream, "\n");
		}
		fprintf(fstream, "%2x:", ptr[i]);
	}
	fprintf(fstream, "\n");
}

uint8_t crc_update(uint8_t oldcrc, uint8_t data)
{
	uint8_t i = 0, crc = 0;

	crc = oldcrc ^ data;

	for (i = 0; i < 8; i++){
		if (crc & 0x01)
			crc = (crc >> 1) ^ 0x8c;
		else
			crc >>= 1;			
	}

	return crc;
}

void crc_set(uint8_t *stream)
{
	uint8_t crc = 0, i, data_len = 0, total_len = 0;
	
	data_len = stream[5];
	total_len = 9 + data_len;
	for ( i = 2 ; i< total_len - 3; i++ ) {
		crc = crc_update(crc, stream[i] );	
	}

	stream[total_len - 3] = crc;
}

uint16_t find_packet_stream(uint8_t *ptr, 
				uint16_t offset,
				uint16_t len, 
				uint8_t *dst)
{
	uint16_t i = offset;

	for ( i = offset; i<len; i++ ){
		uint8_t data_len = 0, total_len = 0;

		data_len = ptr[i + INDEX_LEN];
		total_len = INDEX_DAT + data_len + 3;

		if ( STX0 != ptr[i] ||
			STX1 != ptr[i+1]) {
			continue;
		}
		
		if ( ETX0 != ptr[i + total_len - 2] || 
			ETX1 != ptr[i + total_len - 1] ) {
			continue;
		}	

		memset(dst, 0, MAX_PACKET_LEN);
		memcpy(dst, ptr+i, total_len);

		offset += total_len;
		break;	
	}

	if ( i < len ) {
		return offset;
	} else {
		return i;
	}

	return i;
}

status_t validate_packet_stream(uint8_t *ptr)
{
	uint8_t i = 0, data_len = 0, total_len = 0;
	uint8_t crc = 0, chs = 0;

	data_len = ptr[INDEX_LEN];
	total_len = data_len + INDEX_DAT + 3;
	chs = ptr[INDEX_DAT + data_len];

	if ( RID_HOST != ptr[INDEX_RID] ) {
		return WRONG_RID;
	}

	for ( i = INDEX_RID; i< total_len-3; i++ ) {
		crc = crc_update(crc, ptr[i]);
	}

	if ( crc != chs ) {
		return WRONG_CHECKSUM;
	}

	return SUCCESS;
}
